#include <stdio.h>
#include "../../include/sensors/l3g4200d.h"

/* TODO vérifier ce qu'il y a à "reseter" */
void reset_values_gyr(gyr_sensor *gyr)
{
	gyr->calibration.scale[0] = 1;
	gyr->calibration.scale[1] = 1;
	gyr->calibration.scale[2] = 1;
	gyr->calibration.bias[0] = 0;
	gyr->calibration.bias[1] = 0;
	gyr->calibration.bias[2] = 0;
	gyr->calibration.variance[0] = 50;
	gyr->calibration.variance[1] = 50;
	gyr->calibration.variance[2] = 50;
}

int Gyr_Init(gyr_sensor *gyr){
    FILE *fgyr;
    int errno;
    fgyr = fopen(GYR_Name_file, "r"); /* TODO attention il s'agit du nom pas d'ID de dispo... peut etre le meme que accéléromètre */
    if(fgyr < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fscanf(fgyr, "%s",gyr->name);
    printf("sensor name: %s\n",gyr->name);
    fclose(fgyr);

    fgyr = fopen(GYR_Mode_file, "w+");
    if(fgyr < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fprintf(fgyr,"%d",L3G4200D_PM_ON);
    fclose(fgyr);
    gyr->config.mode=L3G4200D_PM_ON;
    fgyr = fopen(GYR_Rate_file, "r");
    if(fgyr < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    if((fscanf(fgyr, "%x",&gyr->config.rate))<1)
        printf("failed to read sensor rate");
    fclose(fgyr);
    fgyr = fopen(GYR_Range_file, "r");
    if(fgyr < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    if((fscanf(fgyr, "%d",&gyr->config.range)<1))
        printf("failed to read sensor range");
    fclose(fgyr);
    return 0;
}

/* structure de donées pour la configuration du MEMs (rate, gain, mode, ...) */
/* TODO revoir les IF sans doute il faut mettre des else */
int Gyr_write_config(gyr_sensor *gyr)
{
    FILE *fgyr;
    int errno;
    fgyr = fopen(GYR_Mode_file, "w+");
    if(fgyr < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fprintf(fgyr,"%d",gyr->config.mode);
    fclose(fgyr);

    fgyr = fopen(GYR_Rate_file, "w+");
    if(fgyr < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fprintf(fgyr,"%d",gyr->config.rate);
    fclose(fgyr);

    fgyr = fopen(GYR_Range_file, "w+");
    if(fgyr < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fprintf(fgyr,"%d",gyr->config.range);
    fclose(fgyr);
    /* pas besoin d'écrire le gain il se met automatiquement à jour (normalement) */
    return 0;
}

int Gyr_read_config(gyr_sensor *gyr)
{
    FILE *fgyr;
    int errno;
    fgyr = fopen(GYR_Name_file, "r");
    if(fgyr < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fscanf(fgyr, "%s",gyr->name);
    printf("sensor name: %s",gyr->name);
    fclose(fgyr);

    fgyr = fopen(GYR_Mode_file, "r");
    if(fgyr < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    if((fscanf(fgyr, "%d",&gyr->config.mode))<1)
        printf("failed to read sensor rate\n");
    fclose(fgyr);

    fgyr = fopen(GYR_Range_file, "r");
    if(fgyr < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    if((fscanf(fgyr, "%d",&gyr->config.range))<1)
        printf("failed to read sensor range\n");
    fclose(fgyr);

    fgyr = fopen(GYR_Rate_file, "r");
    if(fgyr < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    if((fscanf(fgyr, "%d",&gyr->config.rate))<1)
        printf("failed to read sensor rate\n");
    fclose(fgyr);
    return 0;
}

/*TODO fonction de lecture des données du gyrnétomètre */
/* verifier que la data est bien dispo sinon en renvoie la même donnée
    peut se faire sans doute avec IRQ, à étudier... */
int Gyr_read_data(gyr_sensor *gyr)
{
    int errno,ret;
    int X,Y,Z;
    FILE *fgyr;
    /* verifier que le gyrnétomètre est en marche ??? ou faire dans fonction au dessus... */
    fgyr = fopen(GYR_Data_file, "r");
    if(fgyr<0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fseek(fgyr, 0, SEEK_SET);
    ret = fscanf(fgyr, "%x:%x:%x", &X,&Y,&Z);//&gyr->raw.axis[0], &gyr->raw.axis[1], &gyr->raw.axis[2]);
    if (ret<3)
    {
    	printf("failed to read data gyr\n");
    	return -1;
	}
	gyr->raw.axis[0] = X; /* TODO: verifier la formule ... */
	gyr->raw.axis[1] = Y;
	gyr->raw.axis[2] = Z;
    /*
    printf("gyr data x: %x\t",gyr->raw.axis[0]);
    printf("gyr data y: %x\t",gyr->raw.axis[1]);
    printf("gyr data z: %x\t\n",gyr->raw.axis[2]);
    */
    fclose(fgyr);
    return 0;
}
